#
# Project:    RoboBerDuino
# Author:       Marco Tili
#
# Platform:    Raspberry Pi Model B
#
# Filename:            TBD
# Version:           00.01
#
# Description:
#      TBD
#
#
#
# Changelog:
#       2013-03-22   | M.Tili
#                    | First Issue
#       2013-04-17   | M.Tili
#                    | Fixing for byte mode in tx/rx (not complete)
#                    | Fix for brutal disconnection of the client
#
#
#

from select import select
import Queue
import socket
import sys

class TaskEth():
    connState  = False
    connection = ''
    serverAddress = ''
    sock       = ''
    rx_buff    = None
    tx_buff    = None
    rx_q       = None
    tx_q       = None
    rx_fsm     = 0
    

    def __init__(self,rx_q,tx_q):
        self.rx_q = rx_q
        self.tx_q = tx_q
        
    def Create(self,address,port):
        # Create a TCP/IP socket
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.serverAddress = (address,port)
        
    def StartListen(self,nrClient):
        ret = False
        if not self.connState:
            # Bind the socket to the port
            self.sock.bind(self.serverAddress)
            # Listen for incoming connections
            self.sock.listen(nrClient)
            ret = True
        return ret
    def CheckForConnection(self):
        if not self.connState:
            enR,enW,enErr = select([self.sock],[],[],2)
            if enR:
                self.connection, self.serverAddress = self.sock.accept()
                self.connState = True
        return self.connState
    def DoRX(self):
        ret = False
        data =''        
        if self.connState:
            #try:
            #http://docs.python.org/2/howto/sockets.html#non-blocking-sockets
            ready_to_read, ready_to_write, in_error = select([self.connection],[],[],1)
            if ready_to_read:
                try:
                    data = self.connection.recv(16)
                except:
                    data = None # force closing
                if data:
                    #print >>sys.stderr, "RX: " + data
                    ret = True
                else:
                    # Clean up the connection
                    self.connState = False
                    print "Client disconnected. (1)"
                    self.connection.close()
            #finally:
            # Clean up the connection
            #self.connState = False
            #print "Client disconnected. (2)"
            # self.connection.close()
        return ret,data
    def CloseConnection(self):
        if self.connState:
            # Clean up the connection
            self.connState = False
            print "Client disconnected."
            self.connection.close()
        return self.connState
    def IsConnected(self):
        return self.connState
    def IsConnAlive(self):
        ret = self.CheckForConnection()    
        return self.connState
    def GetClientInfo(self):
        ret1 = ''
        ret2 = ''
        if self.connState:
            ret1 = self.connection
            ret2 = self.serverAddress
        return ret1,ret2
    
    def UpdateTx(self):
        ret = False
        if self.connState and not self.tx_q.empty():
            try:
                tmp_code,tmp_body = self.tx_q.get(0)
                self.DoTX(self.PackMessage(tmp_code,tmp_body))
                ret = True
            except:
                ret = False
        return ret

    def DoTX(self,data):
        try:
            self.connection.sendall(data)           
            print "DEBUG: TX COMPLETE!", data
        except:
            self.connection.close()
            self.connState = False
    

    def PackMessage(self,code,body):
        frame =  ['<','$','>',chr(code)] 
        frame += [chr(i) for i in body] + [chr(170), chr(187)]
        return frame

    def UnpackMessage(self,frame): # type(frame) and type(buff_tx) must be int 
        rx_complete = False
        print "DEBUG: rx_fsm [IN] =", self.rx_fsm        
        for byte in frame:
            print "DEBUG: byte =", byte
            if self.rx_fsm==0:
                self.rx_buff=[]
                if byte==ord('<'):
                    self.rx_buff.append(byte)
                    print "# DEBUG 1 -", self.rx_buff
                    self.rx_fsm = 1
            elif self.rx_fsm==1:
                if byte==ord('$'):
                    self.rx_buff.append(byte)
                    print "# DEBUG 2 -", self.rx_buff
                    self.rx_fsm = 2
                else: #elif byte!=ord('<')
                    self.rx_fsm  = 0
                    self.rx_buff = []
            elif self.rx_fsm==2:
                if byte==ord('>'):
                    self.rx_buff.append(byte)
                    print "# DEBUG 3 -", self.rx_buff
                    self.rx_fsm = 3
                else:
                    self.rx_fsm  = 0
                    self.rx_buff = []
            elif self.rx_fsm==3:
                #header
                self.rx_buff.append(byte)
                if len(self.rx_buff)>=6:
                    length = self.rx_buff[4]
                    length = (length<<8) +self.rx_buff[5]
                    if length!=0:
                        self.rx_fsm=4
                    else:
                        self.rx_fsm=5
            elif self.rx_fsm==4:
                #body
                length = self.rx_buff[4]
                length = (length<<8) +self.rx_buff[5]
                self.rx_buff.append(byte)
                if len(self.rx_buff)>=(6+length):
                    self.rx_fsm = 5
            elif self.rx_fsm==5:
                #footer
                length=self.rx_buff[4]
                length= (length<<8) +self.rx_buff[5]
                self.rx_buff.append(byte)
                if len(self.rx_buff)>=(6+length+2):
                    # FIXME check CRC
                    rx_complete = True
                    print "DEBUG: RX COMPLETE!", self.rx_buff
                    if self.rx_q:
                        # put : [code,body]
                        tmp_code = self.rx_buff[HPOS_CODE]
                        tmp_body = [b for b in self.rx_buff[HPOS_DATASTART:HPOS_DATASTART+length]]
                        self.rx_q.put([tmp_code,tmp_body])
                    self.rx_fsm = 0
        return rx_complete


    def UpdateRX(self):
        ret = False
        isRx,buff = self.DoRX()
        if isRx!=False:
            self.UnpackMessage(map(ord,buff)) # type(buff) is str
            ret = True
        return ret


            

if __name__ == "__main__":
    print "TaskEth main (CTRL+C to quit)."
    print "Echoing incoming message"
    address = 'localhost'
    port    = 10000
    commeth=TaskEth(None,None)    
    commeth.Create(address,port)
    commeth.StartListen(1)
    while 1:
        try:
            print "Check for new connection...",address,":",port
            if commeth.CheckForConnection():
                print "New client connected."
                a,b = commeth.GetClientInfo()
                print str(a),str(b)
                while 1:
                    if not commeth.IsConnected():
                        break
                    isRx,msg = commeth.DoRX()
                    if isRx:
                        print "RX: ", msg
                        commeth.DoTX("ECHO"+msg)
        except KeyboardInterrupt:
            break
    print "TaskEth terminated."


